
template<int n>
LearningData<n>::LearningData(double a_alpha, double a_beta, int a_threshold , int _nbMotors  , int _dimPos):
    alpha(a_alpha), beta(a_beta), threshold(a_threshold) , nbMotors(_nbMotors), dimPos(_dimPos), cellNumber(1), maxLevel(0), maxV(0)
{
    //First, we create the sets
    for(int i=0; i <= threshold; i++)
    {
        sets.append(std::set<Cell<n>*>());
    }

    //Then we create the vertices for the first cell
    QList<int> vertices;
    int iMax = 1<<n;
    for(int i=0; i<iMax; i++){
        QList<double> temp;
        for (int j=0; j<n; j++)
            temp.append( ((i & 1<<j) == 0)? 0 : 1 );
        states.append(State<n>(temp));
        for(int j=0 ; j < nbMotors ; j++)
            states.back().cons << 0.5;
        vertices.append(i);
    }

    //We can now create the cell and add it to the map
    Cell<n> *cell = new Cell<n>(this, &tree, 0, vertices, 0);
    insert(cell);
    //We also add it to the tree
    QList<AbstractNode*> list;
    list.append(cell);
    tree.setUnder(list);
}

template<int n>
LearningData<n>::LearningData(QString filename)
{
    QFile file(filename);
    file.open(QIODevice::ReadOnly);
    QTextStream in(&file);

    //We skeep the first four lines
    in.readLine();  //system type
    in.readLine();  //physical parameters
    in.readLine();  //motor parameters
    in.readLine();  //sensor parameters


    //Now we begin

    int nStates = in.readLine().toInt();
    for(int i=0; i<nStates; i++)
    {
        QStringList line = in.readLine().split(':', QString::SkipEmptyParts);
        QList<double> params;
        for(int j = 1 ; j < n+1 ; j++){
                params << line[j-1].toDouble();
        }

        states.append(State<n>(params));
        for(int j = n ; j < line.size() ; j++)
            states[i].cons << line[j].toDouble();
    }

    maxLevel = in.readLine().toInt();

    reconstruct();
}

template<int n>
void LearningData<n>::exportData(QString filename)
{
    QFile file(filename);
    if(!file.open(QIODevice::WriteOnly | QIODevice::Text | QIODevice::Append))
    {
        qDebug() << "Cannot open : " << filename<<" for exportation" << endl;
    }

    QTextStream out(&file);
    out<<states.size()<<endl;
    for(int i=0; i<states.size(); i++)
    {
        for(int j=0; j < n; j++)
        {
            out<<states[i].params[j]<<":";
        }
        for(int j=0; j < states[0].cons.size(); j++)
        {
            out<<":" << states[i].cons[j];
        }
        qDebug()<<states[i].params <<":" << states[i].cons;
        out << endl;
    }

    out<<maxLevel<<endl;
}

template<int n>
void LearningData<n>::reconstruct()
{
    //First, let's create sets to store the keys of the nodes
    QList<std::set<Key<n> > > keySets;
    for(int i=0; i <= maxLevel; i++)
    {
        keySets.append(std::set< Key<n> >());
    }


    //Now, let's search for cells


    //We'll start by sorting the states
    std::sort(states.begin(), states.end());

    //Then, we can use the structure to find cells
    int size = states.size();
    int maxCount = 1 << n;
    double precision = 1.0/static_cast<double>(1<<(maxLevel+2));

    for(int i=0; i < size-1; i++)
    {
        QList<int> temp;
        temp.append(i);
        int last = 0, limit = 1, count = 1;

        //If the distance to the next state is less than one, it can be the edge of a cell
        double dist, edgeSize = states[i].distanceTo(states[i+1].params);
        if(edgeSize > 1)
        {
            continue;
        }

        //In that case, we try to complete the cell, using the structure
        for(int j=i+1; ((j < size) && (count < maxCount)); j++)
        {
            dist = states[temp[last]].distanceTo(states[j].params);
            if( (dist <= edgeSize + precision) && (dist >= edgeSize - precision) )
            {
                last++;
                temp.append(j);
                count ++;

                if(last == limit)
                {
                    last = 0;
                    limit = count;
                }
            }
        }

        //If we have found all edges, we add the cell and attach it to a node
        if (count == maxCount)
        {
            int level = static_cast<int>( floor( -log(edgeSize-precision)/log(2) ) );

            Cell<n> *cell = new Cell<n>(this, 0, 0, temp, level);
            insert(cell);

            if(level > 0)
            {
                Key<n> key(level-1, barycenter(temp).params);
                Node* node;
                if(keySets[level-1].count(key) == 0)
                {
                    node = new Node;
                    lookupMap.insert(key, node);
                    keySets[level-1].insert(key);
                }
                else
                {
                    node = (Node*)(lookupMap[key]);
                }

                cell->setNode(node);
            }
        }
    }

    //When all cells have been found and attributed to nodes, we recreate the whole tree
    for(int i=maxLevel-1; i>0; i--)
    {
        //At each level, we attach the nodes to corresponding lower level nodes
        typedef typename std::set<Key<n> >::iterator setIterator;
        for(setIterator it=keySets[i].begin(); it != keySets[i].end(); it++)
        {
            Key<n> key = *it;
            Node *currentNode = (Node*)(lookupMap[key]);
            key.level--;
            for(int j=0; j<n; j++)
            {
                key.params[j] /= 2;
            }

            Node *newNode;
            if(keySets[i-1].count(key) == 0)
            {
                newNode = new Node;
                lookupMap.insert(key, newNode);
                keySets[i-1].insert(key);
            }
            else
            {
                newNode = (Node*)(lookupMap[key]);
            }

            newNode->insert(currentNode);
        }
    }

    //We finish by storing the lowest node/cell in tree
    QList<double> params;
    for(int i=0; i<n; i++)
    {
        params.append(0);
    }

    tree.insert(lookupMap[Key<n>(0,params)]);
}

template<int n>
inline int LearningData<n>::insert(State<n> s)
{
    int result = states.size();
    states.append(s);
    return(result);
}

template<int n>
inline void LearningData<n>::insert(Cell<n> *cell)
{
    lookupMap[Key<n>(cell->getLevel(), cell->getBarycenter().params)] = cell;

    if(sets.size() != 0)
    {
        sets[0].insert(cell);
    }

    cellNumber++;
}

template<int n>
inline void LearningData<n>::replace(Cell<n> *cell, Node *node)
{
    lookupMap[Key<n>(cell->getLevel(), cell->getBarycenter().params)] = node;

    if(sets.size() != 0)
        sets[cell->getVisits()].erase(cell);

    delete(cell);
    cellNumber--;
}

template<int n>
inline void LearningData<n>::visit(Cell<n> *cell)
{
    int i=cell->getVisits();
    if(i < threshold)
    {
        sets[i].erase(cell);
        sets[i+1].insert(cell);
        cell->incrementVisits();
    }
}

template<int n>
inline Cell<n>* LearningData<n>::getLessVisited()
{
    for(int i=0; i < threshold; i++)
        if(sets[i].size() != 0)
            return ( *(sets[i].begin()) );

    return(0);
}

template<int n>
inline int LearningData<n>:: getToVisit(){
    int res = 0;
    for(int i=0; i < threshold; i++)
       res += sets[i].size();
    return res;
}

template<int n>
inline State<n> LearningData<n>::barycenter(QList<int> list)
{
    QList<double> params;
    double size = list.size();

    //First, we take the mean of the parameters
    double temp;
    for (int i=0; i<n; i++)
    {
        temp = 0;
        for(int j=0; j<size; j++)
        {
            temp += states[list[j]].params[i];
        }
        temp /= size;
        params.append(temp);
    }

    //We use them to create a new state
    State<n> result(params);

    //We choose V as the mean V
    temp = 0;
    for(int j=0; j<size; j++)
    {
        temp += states[list[j]].V;
    }
    temp /= size;
    result.V = temp;

    //And Cons
    for(int i = 0 ; i < states[list[0]].cons.size() ; i++){
        temp = 0;
        for(int j=0; j<size; j++)
        {
            temp += states[list[j]].cons[i];
        }
        temp /= size;
        result.cons << temp;
    }

    //We can now return the state
    return(result);
}

template<int n>
inline QList<double> LearningData<n>::randomParams(QList<int> list)
{
    //Generate a random ponderation
    int size = list.size();
    double weight[size] , tot = 0;
    srand(time(NULL));
    for(int i = 0 ; i < size ; i++){
        weight[i] = rand()/((double)RAND_MAX);
        tot += weight[i];
    }

    //We take the ponderate barycenter of the parameters
    QList<double> params;
    for (int i=0; i<n; i++)
    {
        params << 0;
        for(int j=0; j<size; j++)
            params.back() += weight[j] * states[list[j]].params[i] / tot;
    }

    return(params);
}

template<int n>
Cell<n>* LearningData<n>::getCell(QList<double> list)
{
    int levelMin = 0;
    int levelMax = maxLevel;

    AbstractNode *result;

    Key<n> temp(levelMin, list);
    if(lookupMap.count(temp) == 1)
    {
        result = lookupMap[temp];
        if(result->isCell())
        {
            return((Cell<n>*)(result));
        }
    }

    temp = Key<n>(levelMax, list);
    if(lookupMap.count(temp) == 1)
    {
        result = lookupMap[temp];
        if(result->isCell())
        {
            return((Cell<n>*)(result));
        }
    }

    int currentLevel = (levelMin + levelMax)/2;
    temp = Key<n>(currentLevel, list);

    while(true)
    {
        if(lookupMap.count(temp) == 1)
        {
            result = lookupMap[temp];
            if(result->isCell())
            {
                return((Cell<n>*)(result));
            }
            levelMin = currentLevel;
        }
        else
        {
            levelMax = currentLevel;
        }
        currentLevel = (levelMin + levelMax)/2;
        temp = Key<n>(currentLevel, list);
    }

}
